#include <iostream>

#if 0

#include "urcontrolplane.hpp"


int main(int argc, char *argv[])
{
    if(argc!=10)
    {
        std::cout<<"parameters error\n";
        return 1;
    }

    cv::Vec2d cornerPixel[4];
    double openmm=50;

    std::istringstream(argv[1])>>cornerPixel[0][0];
    std::istringstream(argv[2])>>cornerPixel[0][1];
    std::istringstream(argv[3])>>cornerPixel[1][0];
    std::istringstream(argv[4])>>cornerPixel[1][1];
    std::istringstream(argv[5])>>cornerPixel[2][0];
    std::istringstream(argv[6])>>cornerPixel[2][1];
    std::istringstream(argv[7])>>cornerPixel[3][0];
    std::istringstream(argv[8])>>cornerPixel[3][1];
    std::istringstream(argv[9])>>openmm;

    URControlPlane urcp;

    urcp.PickRectangleCenter(cornerPixel, "192.168.1.13","192.168.1.14", openmm);

    return 0;
}
#endif


#if 0

#include "urcontrol.hpp"

int main()
{
//    URControl::UR5Demo();
//    ToST().test();

    URControl::test();

//    URScript::RGZOffset();
    return 0;
}

#endif


#if 0

#include "urcontroloffseta.hpp"

int main(int argc, char *argv[])
{
    URControlOffsetA urco;

//    URControlOffset::Calibrate2();


    urco.test();
    return 0;
    if(argc!=8)
    {
        std::cout<<"parameters error\n";
        return 1;
    }

    double openmm=5;
    double zOffset=0;
    double x=0;
    double y=0;
    double z=0;
    double xyRoll=0;
    double v=0.2;

    std::istringstream(argv[1])>>openmm;
    std::istringstream(argv[2])>>zOffset;
    std::istringstream(argv[3])>>x;
    std::istringstream(argv[4])>>y;
    std::istringstream(argv[5])>>z;
    std::istringstream(argv[6])>>xyRoll;
    std::istringstream(argv[7])>>v;
//    std::istringstream(argv[8])>>cornerPixel[3][1];

    urco.PickPoint(openmm, zOffset, x, y, z, xyRoll, 0.1, v, false);

    return 0;
}

#endif

#if 0

#include "urcontrolcamera.hpp"

int main(int argc, char *argv[])
{
    URControlCamera urcc;

//    urcc.Calibrate();

//    urcc.test();
    urcc.play();
//    urcc.c.Calib("/home/pi/ur3/20171121_151132/");
//    urcc.CalibratePosition();
//    urcc.CalibratePlane();

//    urcc.Sample();

//    urcc.SampleOne();


//    cv::Vec2d xy[4];
//    cv::Vec3d xyz[4];
//    urcc.CodeCorners("C:/Users/jhanbin/Desktop/pro/ur3/build-urct-Desktop_Qt_5_8_0_MinGW_32bit-Release/20171128_113525/20171128113731.png",xy,xyz);

//    urcc.LoadCorners("20180406_104948/");
//    urcc.LoadCorners("C:/Users/jhanbin/Desktop/pro/ur3/build-urct-Desktop_Qt_5_8_0_MinGW_32bit-Release/20171128_130639/");

//    urcc.LoadCorners("C:/Users/jhanbin/Desktop/pro/ur3/build-urct-Desktop_Qt_5_8_0_MinGW_32bit-Release/20171129_100238/");

//    urcc.LoadCorners("C:/Users/jhanbin/Desktop/pro/ur3/build-urct-Desktop_Qt_5_8_0_MinGW_32bit-Release/20171129_102144/");
//urcc.LoadCorners("C:/Users/jhanbin/Desktop/pro/ur3/build-urct-Desktop_Qt_5_8_0_MinGW_32bit-Release/20171129_103317/");

//    urcc.LoadCorners("C:/Users/jhanbin/Desktop/pro/ur3/build-urct-Desktop_Qt_5_8_0_MinGW_32bit-Debug/20171204_093751/");

    return 0;
}

#endif

#if 0

#include "urcontrolcamera.hpp"

int main(int argc, char *argv[])
{
    char *ipUR="192.168.1.13";
    char *ipPC="192.168.1.14";

    if(argc!=10)
    {
        std::cout<<"parameters error\n";
        return 1;
    }

    cv::Vec2d cornerPixel[4];
    double v=0.2;

    std::istringstream(argv[1])>>cornerPixel[0][0];
    std::istringstream(argv[2])>>cornerPixel[0][1];
    std::istringstream(argv[3])>>cornerPixel[1][0];
    std::istringstream(argv[4])>>cornerPixel[1][1];
    std::istringstream(argv[5])>>cornerPixel[2][0];
    std::istringstream(argv[6])>>cornerPixel[2][1];
    std::istringstream(argv[7])>>cornerPixel[3][0];
    std::istringstream(argv[8])>>cornerPixel[3][1];
//    std::istringstream(argv[9])>>v;

    URControlCamera urcc;

    double openmm;

    urcc.PickRectangle(openmm, cornerPixel,cv::Vec6d(0,0,0.21,0,0,CV_PI/4),ipUR,ipPC,0.1,v);

    return 0;
}

#endif

#if 1

#include <opencv2/opencv.hpp>

int main()
{
    cv::Vec3d pt;

    double beta=CV_PI*20/180;

    cv::Matx33d rmx;
    cv::Rodrigues(cv::Vec3d(beta, 0, 0), rmx);

    cv::Matx33d rmz;

     cv::Rodrigues(cv::Vec3d(0, 0, CV_PI/2), rmz);

     cv::Vec3d offset(12+14+13*cos(beta)+24*sin(beta), -32, 34+24+12-13*sin(beta)+24*cos(beta));


     cv::Vec3d pto=rmz*rmx*pt+offset;




    return 0;
}

#endif
